/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/*
*  \class	TCoord2D
*  \author 	Jari Saarinen
*  \date 	Fri Mar 10 12:26:30 2006

*  \version: 	0.1  
*/
#include <math.h>
#include <stdio.h>
#include "LNMdatastructs.h"
#include "LnM_math.h"		// LnMLib math utility functions
#include "TCoord2D.h"



//-------------------------------------------------------------//
TCoord2D::TCoord2D()
{
	this->x = 0;
	this->y = 0;

}

TCoord2D::TCoord2D(float x, float y)
{
	this->x = x;
	this->y = y;

}

TCoord2D::~TCoord2D(){}

//-------------------------------------------------------------//
// Operators
//-------------------------------------------------------------//
TCoord2D TCoord2D::operator +(TCoord2D a) const{
	return TCoord2D(a.x+x,a.y+y);
}

TCoord2D TCoord2D::operator -(TCoord2D a) const{
	return TCoord2D(a.x-x,a.y-y);
}
//-------------------------------------------------------------//
// Interface
//-------------------------------------------------------------//

/**
* Return the X-coordinate
*/
float TCoord2D::getX(){
	return this->x;
}
/**
* Return the Y-Coordinate
*/
float TCoord2D::getY(){
	return this->y;
}

/**
* Returns the R of the polar coordinates
*/
float TCoord2D::getPolarDist(){
	return (float)sqrt(this->x*this->x+this->y*this->y);
}

/**
* Returns the Angle from the polar coordinates
*/
float TCoord2D::getPolarAngle(){
	return (float)atan2(this->y,this->x);
}

/**
* Set X and Y Coordinates
*/
void TCoord2D::setCoord(float x,float y){
	this->x = x;
	this->y = y;
}

/**
* Makes addition
*/
void TCoord2D::addCoord(float x, float y){
	this->x+=x;
	this->y+=y;
}

/**
* Adds a polar coordinates to current coordinates
*/
void TCoord2D::addPolar(float r,float a){
	float dx;
	float dy;

	dx = r * (float)cos(a);
	dy = r * (float)sin(a);

	this->x += dx;
	this->y += dy;
}

/**
* Rotates this point around coordinate P
* @param P Coordinate around which the rotation is done
* @param alpha rotation angle
*/
void TCoord2D::rotate(TCoord2D P, float alpha){
	float cos_alpha = (float)cos(alpha);
	float sin_alpha = (float)sin(alpha);
	
	this->x = cos_alpha * (x - P.getX()) - sin_alpha * (y - P.getY()) + P.getX();
	this->y = sin_alpha * (x - P.getX()) + cos_alpha * (y - P.getY()) + P.getY(); 
}

/**
* Translates this P amount
* @param P  the amount which the coordinate should be translated
*/
void TCoord2D::translate(TCoord2D P){
    this->x += P.getX();
    this->y += P.getY();
}


/**
* Transforms the TCoord2D value to pixel coordinates. The TCoord value and TPixelCoord values are 
* represented in Global coordinate system
* @param resolution		The transformation resolution 
* @return				The transformed pixel Cooddinate
**/
TPixelCoord  TCoord2D::toPixelCoord(float resolution){
	TPixelCoord P;
	
	if(resolution <= 0) {
		fprintf(stderr,"TCoord2D::toPixelCoord(float resolution):: Invalid Resolution!\n");
		P.x = 0;
		P.y = 0;
	}else{
		
		P.x = lnm_roundf( this->x / resolution);
		P.y = lnm_roundf( this->y / resolution);
	}

	return P;
}

/**
* Transforms the TCoord2D value to relative/BasicGrid coordinate system. The 
* TCoord2d value is given in the global (right-hand cartesian) coordinate system in meters
* The resolution is the grid size in meters and the upLeft is the coordinate of the 
* given grid upper left corner. 
* The output is the pixel coordinate in the "basic grid" coordinate system 
* (origin is the upper left corner, y-down and x-right)
* 
* @param resolution		The transformation resolution 
* @param upLeft			The upper left corner pixel coordinate in global frame
* @return				The value in given grid coordinates 
*/
TPixelCoord  TCoord2D::toRelativePixelCoord(float resolution, TPixelCoord upLeft){
	TPixelCoord glob;
	TPixelCoord rel;

	glob = this->toPixelCoord(resolution);	// To global pixels
	
	rel.x = -upLeft.x + glob.x;
	rel.y = upLeft.y - glob.y;

	return rel;
}


//----------------------------------------------------------------//
